/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/PositionTarget.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TwistStamped.h>
#include <math.h>
#include <std_srvs/Trigger.h>
#include <std_msgs/ColorRGBA.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf/transform_datatypes.h>
#include <mavros_msgs/PositionTarget.h>
#include <std_msgs/Int8.h> //some includes
#include <stdlib.h>


#define TargeyHeight 1.47

//订阅全局变量
ros::Subscriber state_sub;//飞机当前状态
ros::Subscriber pose_sub;//飞机当前位置信息
ros::Subscriber vel_sub;//飞机速度信息
ros::Subscriber data_receive;//接收python节点的数据
ros::Publisher local_pose_pub;//设置飞机目标位置（全局坐标
ros::Publisher vel_pub;//设置飞机速度（全局坐标
ros::Publisher state_pub;//飞机状态
ros::Publisher set_raw_pub; //以飞机自己为坐标系的速度控制
ros::Publisher hit_pub;
ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;
geometry_msgs::TwistStamped vel_cmd; //全局消息，速度设置
geometry_msgs::PoseStamped pose,land_pose;//全局消息，分别用来控制飞机自身的位置与飞机降落的位置
mavros_msgs::PositionTarget raw_pose; //全局消息，以飞机自己为坐标系
bool cur_state = false;
double yaw_last_time;

std_msgs::ColorRGBA py_data; //用于接收python节点的数据
void receive(const std_msgs::ColorRGBAConstPtr& msg){
	py_data = *msg;
}

bool state_cb(std_srvs::Trigger::Request &req,std_srvs::Trigger::Response &res){  //用于通过命令启动飞机
    cur_state=!cur_state;
    // 显示请求数据
    ROS_INFO("Publish command [%s]", cur_state==true?"Yes":"No");
	// 设置反馈数据
	res.success = true;
	res.message = "Change drone command state!";
    return true;
}

double min(double a,double b){   //比较ab较小的一个数，其中a的值是被比较值，b是限制量（注意，不可用反
    double ret=0;
    if (a == 0){
        ret=0;
    }else if(fabs(a)>=fabs(b)){
        ret=fabs(b)*a/fabs(a);
    }else{
        ret=fabs(a)*a/fabs(a);
    }
    return ret;
}

mavros_msgs::State current_state;  //订阅消息：当前状态
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

geometry_msgs::PoseStamped current_pose; //订阅消息：当前位置（世界坐标系）
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) {
    current_pose = *msg;
}

geometry_msgs::PoseStamped orig;

geometry_msgs::TwistStamped current_vel; //订阅消息：当前速度
void vel_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
    ROS_INFO("Current velocity: %.2f m/s", msg->twist.linear.x);
    current_vel = *msg;
}

double distance(geometry_msgs::PoseStamped cur_pos,geometry_msgs::PoseStamped tar_pos){ //判断亮点距离函数
      double dist=0;
      dist=sqrt(pow((cur_pos.pose.position.x-tar_pos.pose.position.x),2 )+
      pow((cur_pos.pose.position.y-tar_pos.pose.position.y),2 )+
      pow((cur_pos.pose.position.z-tar_pos.pose.position.z),2 ));
      return dist;
}    

// void set_speed_body(double x, double y, double z,  
// double yaw_rate ) //设置飞机的速度，其中x,y,z都是飞机以自己为坐标系的速度，yaw_rate是偏航角旋转速率
// {
// 	mavros_msgs::PositionTarget raw_target;
// 	raw_target.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;
// 	raw_target.type_mask = mavros_msgs::PositionTarget::IGNORE_PX | 
//     mavros_msgs::PositionTarget::IGNORE_PY | 
//     //mavros_msgs::PositionTarget::IGNORE_PZ | 
//     mavros_msgs::PositionTarget::IGNORE_AFX | 
//     mavros_msgs::PositionTarget::IGNORE_AFY | 
//     mavros_msgs::PositionTarget::IGNORE_AFZ | 
//     mavros_msgs::PositionTarget::IGNORE_YAW;
// 	if (fabs(yaw_rate) < 1e-6)
// 	raw_target.type_mask |= mavros_msgs::PositionTarget::IGNORE_YAW_RATE;
//     ROS_INFO("body_speed:%.1lf %.1lf %.1lf",x,y,yaw_rate);
//     raw_target.position.z = 0;
// 	raw_target.velocity.x = x;
// 	raw_target.velocity.y = y;
// 	raw_target.velocity.z = z;
// 	raw_target.yaw_rate =yaw_rate;//yaw_rate * 0.01745329;
// 	set_raw_pub.publish(raw_target);
// }

void pose_velocity_ctrl(double vx, double vy, double vel_limit_x,double vel_limit_y, 
double yaw_rate, double yaw_limit, 
ros::Rate& rate ){   //设置飞机的速度，其中x,y,z都是飞机以世界为坐标系的速度，yaw_rate是偏航角旋转速率
    geometry_msgs::PoseStamped position=current_pose;
    // double current_time = ros::Time::now().toSec();
    // double elapsed_time = current_time - yaw_last_time;
    // yaw_last_time = current_time;
    double dt = rate.expectedCycleTime().toSec();
    vx=min(vx,vel_limit_x);
    vy=min(vy,vel_limit_y);
    yaw_rate = min(yaw_rate,yaw_limit);
    position.header.stamp = ros::Time::now();
    position.header.frame_id = "map";
    ROS_INFO(" vx = %.2lf vy = %.2lf " ,vx , vy);
    //double current_yaw = tf::getYaw(current_pose.pose.orientation);// Set the new yaw angle
    //double new_yaw = current_yaw + yaw_rate * dt;
    //tf::Quaternion q = tf::createQuaternionFromYaw(new_yaw);
    position.pose.position.x = current_pose.pose.position.x + vx*dt;
    position.pose.position.y = current_pose.pose.position.y + vy*dt;
    position.pose.position.z = TargeyHeight; 
    position.pose.orientation.x = orig.pose.orientation.x;
    position.pose.orientation.y = orig.pose.orientation.y;
    position.pose.orientation.z = orig.pose.orientation.z;
    position.pose.orientation.w = orig.pose.orientation.w;
    local_pose_pub.publish(position);
}

double low_pass_fliter(double x){ //简易滤波器，取过去50次的平均值
    static double prev=0;
    double y = 0.95*prev+0.05*x;
    prev = y;
    return y;
}

void line_follower(std_msgs::ColorRGBA RGB,
 double limit_v_x, double limit_v_y ,double limit_a, ros::Rate& rate){  //巡线函数，已经调试完成
    double rgbr = RGB.r;
    double rgba = RGB.g*3.1415/180;
    double kp = 0.5;
    double ki = 0.08;
    double kd = 0;
    double ir = 0.0, ia = 0.0;
    double er = 0.0, ea = 0.0;
    double last_r = 0.0, last_a = 0.0;      // 计算PID控制器输出
    double dt = rate.expectedCycleTime().toSec();
    er = min(rgbr,300);
    er = rgbr*0.1;
    //er = low_pass_fliter(er);
    ea = min(rgba,0.52);
    ir += er * dt;
    ia += ea * dt;
    double dr = (er - last_r) / dt;
    double da = (ea - last_a) / dt;
    double vr = kp * er + ki * ir + kd * dr;
    double va = kp * ea + ki * ia + kd * da;
    last_r = er;
    last_a = ea;
    double vx=0;
    double vy=0;
    if(ea == 0 || er == 0){
        vx = 0;
        vy = limit_v_y;
    }else{
        vy = fabs(vr/sin(2*ea)*sin(1.57075-3*ea));
        //vx = (-er)/fabs(sin(2*ea)*cos(1.57075-3*ea));
        vx = fabs(vy*tan(3*ea))*fabs(er)/(-er);
    }
    //double current_yaw = tf::getYaw(current_pose.pose.orientation);
    //ROS_INFO("b=%.2lf r=%.2lf g=%.2lf" ,py_data.b, py_data.r ,py_data.g);
    //set_speed_body(min(vx,limit_v),min(vy,limit_v),0,min(va,limit_a));
    pose_velocity_ctrl(vx,vy,limit_v_x,limit_v_y,0,0,rate);
    //velocity_ctrl(vx,vy,va,limit_v,limit_a,vel_pub); //-va*100
}

int circle_locator(std_msgs::ColorRGBA RGB,
double limit_v_x,double limit_v_y ,ros::Rate& rate){  //对准圆心函数
    static int a = 0; 
    //ROS_INFO(" err_x =%.2lf err_y = %.2lf",RGB.r , RGB.g);
    double err_x = -RGB.r;  
    double err_y = RGB.g+36;
    double kp = 0.05;
    double ki = 0.004;
    double ix = 0.0, iy = 0.0;
    double ex = 0.0, ey = 0.0;
    double dt = rate.expectedCycleTime().toSec();
    ex = min(err_x,200);
    ey = min(err_y,200);
    //ex = low_pass_fliter(ex);
    //ey = low_pass_fliter(ey);
    iy += ey * dt;
    ix += ex * dt;
    double vy = kp * ey + ki * iy ;
    double vx = kp * ex + ki * ix ;
    ROS_INFO("erx=%.2lf ery=%.2lf" ,ex,ey);
    pose_velocity_ctrl(vx,vy,limit_v_x,limit_v_y,0,0,rate);
    if(fabs(vx) < 0.3 && fabs(vy) <0.3 && a == 0){
        a = 1;
    }
    return a;
}


// bool fly_to_target(geometry_msgs::PoseStamped target_pose,
// double limit, ros::Publisher& local_vel_pub,
//  ros::Rate& rate) {
//     bool judge=true;
//     // 初始化PID控制器
//     double kp = 0.05;
//     double ki = 0.04;
//     double kd = 0.04;
//     double ix = 0.0, iy = 0.0, iz = 0.0;
//     double ex = 0.0, ey = 0.0, ez = 0.0;
//     double last_x = 0.0, last_y = 0.0, last_z = 0.0;      
//         // 计算PID控制器输出
//         double dt = rate.expectedCycleTime().toSec();
//         ex = target_pose.pose.position.x - current_pose.pose.position.x;
//         ey = target_pose.pose.position.y - current_pose.pose.position.y;
//         ez = target_pose.pose.position.z - current_pose.pose.position.z;
//         ix += ex * dt;
//         iy += ey * dt;
//         iz += ez * dt;
//         double dx = (ex - last_x) / dt;
//         double dy = (ey - last_y) / dt;
//         double dz = (ez - last_z) / dt;
//         double vx = kp * ex + ki * ix + kd * dx;
//         double vy = kp * ey + ki * iy + kd * dy;
//         double vz = kp * ez + ki * iz + kd * dz;
//         last_x = ex;
//         last_y = ey;
//         last_z = ez;

//         // 更新速度指令
//         geometry_msgs::TwistStamped vel;
//         vel.header.stamp = ros::Time::now();
//         vel.twist.linear.x = min(vx,limit);
//         vel.twist.linear.y = min(vy,limit);
//         vel.twist.linear.z = min(vz,limit);
//         vel.twist.angular.x = 0.0;
//         vel.twist.angular.y = 0.0;
//         vel.twist.angular.z = 0.0;

//         // 发布速度指令
//         local_vel_pub.publish(vel);
//         ROS_INFO("pos:%.1lf %.1lf %.1lf",
//         current_pose.pose.position.x,
//         current_pose.pose.position.y,
//         current_pose.pose.position.z
//         );
//         // 判断是否到达目标点
//         if (fabs(ex) < 0.1 && fabs(ey) < 0.1 && fabs(ez) < 0.1) {
//             judge = false;
//         }
//         return judge;
// }

int main(int argc, char **argv)
{
    int step=1;
    double target_speed=0.5;// 初始化 ROS 节点
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;
    // 订阅飞行器状态和发布位置设定点
    state_sub = nh.subscribe<mavros_msgs::State>
      ("/mavros/state", 10, state_cb);  //订阅状态
    local_pose_pub = nh.advertise<geometry_msgs::PoseStamped>
      ("/mavros/setpoint_position/local", 10);  //设置位置
    pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
      ("/mavros/local_position/pose", 10, pose_cb); //订阅位置信息.
    vel_sub = nh.subscribe<geometry_msgs::TwistStamped>
       ("/mavros/local_position/velocity", 10 ,vel_cb);
    vel_pub = nh.advertise<geometry_msgs::TwistStamped>
      ("/mavros/setpoint_velocity/cmd_vel", 10);  //设定速度
    data_receive = nh.subscribe<std_msgs::ColorRGBA>
	  ("data",10,receive);
    state_pub = nh.advertise<std_msgs::Int8>
        ("/state" , 1 );
    set_raw_pub = nh.advertise<mavros_msgs::PositionTarget>
       ("/mavros/setpoint_raw/local", 10);
    hit_pub = nh.advertise<std_msgs::Int8>
       ("/hit", 1);   
    // 定义服务客户端，用于解锁/上锁无人机和切换到离线控制模式
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient land_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/land");     
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");
    ros::ServiceServer state_client = nh.advertiseService
            ("/command",state_cb);        
    // 指定发布位置设定点的频率
    ros::Rate rate(20.0);  

    // 等待 FCU 连接
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }
    // 发布一些起始位置设定点，然后才开始执行控制指令
    // 请求进入离线控制模式
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    // 请求解锁飞行器
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    // 记录上一次请求时间
    ros::Time last_request = ros::Time::now();
    mavros_msgs::CommandBool land_cmd;
    std_msgs::Int8 state_call_back;
    std_msgs::Int8 hit_call_back;
    state_call_back.data = 0; //初始化状态反馈，
    hit_call_back.data = 0;  //初始化打靶状态反馈
    int state_judge = 0;
    while(ros::ok()){
        ros::spinOnce(); 
        //state_pub.publish(state_call_back);
        ROS_INFO("step = %d rgb.b = %d state = %d ",step,(int)py_data.b, state_call_back.data);
        if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");
                state_pub.publish(state_call_back);
                hit_pub.publish(hit_call_back);
                pose.pose = current_pose.pose;
                orig.pose = current_pose.pose;
                pose.pose.position.z=TargeyHeight;
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) && arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            } 
            if(cur_state){
                switch(step){
            case 1:
                if( current_pose.pose.position.z < 1.4){
                    pose_velocity_ctrl(0,0,0,0,0,0,rate);
                }else{
                    //orig.pose = current_pose.pose;
                    pose.pose = current_pose.pose;
                    last_request = ros::Time::now();
                    step=2;
                }
                break;
            case 2:
                     switch((int)py_data.b){
                         case 0:
                             pose.pose=current_pose.pose;
                             pose.pose.position.z=TargeyHeight;
                             pose.pose.position.y+=0.01;
                             pose_velocity_ctrl(0,0,0,0,0,0,rate);
                          break;
                         case 1:
                             line_follower(py_data,2,1.7,0,rate);
                          break;
                         case 2:
                             if(state_call_back.data == 0){
                             step = 3;
                             hit_call_back.data = 1;
                             hit_pub.publish(hit_call_back);
                             pose.pose=current_pose.pose;
                             pose.pose.position.z=TargeyHeight;
                             pose_velocity_ctrl(0,0,0,0,0,0,rate);
                             last_request = ros::Time::now();
                             }else{
                             ROS_INFO("error");
                             pose.pose=current_pose.pose;
                             pose.pose.position.z=TargeyHeight;
                             pose_velocity_ctrl(0,0,0,0,0,0,rate);
                             }
                           break;
                         case 3:
                             step = 4;
                             pose.pose=current_pose.pose;
                             pose.pose.position.z=TargeyHeight;
                             pose_velocity_ctrl(0,0,0,0,0,0,rate);
                           break;
                         default:
                             pose.pose=current_pose.pose;
                             pose.pose.position.z=TargeyHeight;
                             pose_velocity_ctrl(0,0,0,0,0,0,rate);
                         break;
                     }
                    last_request = ros::Time::now();
                    break;
            case 3:
                if(ros::Time::now() - last_request <
                   ros::Duration(17.0)){
                    if((int)py_data.b == 2){
                        state_judge = circle_locator(py_data,1.5,1.5,rate);
                     }else{
                        pose.pose=current_pose.pose;
                        pose.pose.position.z=TargeyHeight;
                        pose_velocity_ctrl(0,0,0,0,0,0,rate);
                        //local_pose_pub.publish(pose);
                        
                    }
                }else{
                    step = 2;
                    state_call_back.data = 1;  //飞机完成巡线，state的值变成1
                    hit_call_back.data = 0;  //退出激光制导
                    state_pub.publish(state_call_back);
                    hit_pub.publish(hit_call_back);
                    pose.pose=current_pose.pose;
                    last_request = ros::Time::now();
                }
                break;
            case 4:
             if(ros::Time::now() - last_request <
                   ros::Duration(10.0)){
                    if((int)py_data.b == 3){
                        state_judge = circle_locator(py_data,1.5,1.5,rate);
                     }else{
                        pose.pose=current_pose.pose;
                        pose.pose.position.z=TargeyHeight;
                        pose_velocity_ctrl(0,0,0,0,0,0,rate);
                    }
                }else{
                    step = 6;
                    pose.pose=current_pose.pose;
                    last_request = ros::Time::now();
                }
                break;
            case 5:
                pose.pose.position.z=0.5;
                ROS_INFO("mode5");
                 if(current_pose.pose.position.z >0.6){
                     pose_velocity_ctrl(0,0,0,0,0,0,rate);
                }else{
                    step=6;
                    }
                break;
            case 6:
                land_client.call(land_cmd); 
                arm_cmd.request.value = false;
               if(arming_client.call(arm_cmd) && arm_cmd.response.success){
                ROS_INFO("Vehicle disarmed");
                return 0;
                 }
                //return 0;
                //ros::Duration(1).sleep();
                break;
            default:
                break;
            }
             
        }
        rate.sleep();
        }
    }
    return 0;
}
